/*
 *  robot_sim.h
 *  
 *
 *  Created by Jeff McKinstry on 5/4/09.
 *  Copyright 2009 __The Neurosciences Institute__. All rights reserved.
 *
 *
 *  Wrapper for the robot code.
 *
 */

#include <iostream>
//#include "../robot/ape-x/serbot.h"
using namespace std;

extern MPI_Comm network_comm;

const	double	pi=3.1415926;


const int frame_time_ms = 10;
const int n_motors=3;
const float max_velocity_rad_s = 114.0f/60.0*2*pi; 
const float rad_per_level = 300.0f/1024.0f*pi/180.0f;

#define APEX2

#ifdef APEX1  // (yellow wiring harness.)

#define ELBOW 3
#define DELTOID 5
#define SHOULDER 6
// shoulder, deltoid, elbow
const float mins[]={200, 380, 305};
const float maxs[]={512, 657, 687};

#else // (blue wiring harness.)

#define HAND 0
#define ELBOW 4
#define DELTOID 6
#define SHOULDER 7
// shoulder, deltoid, elbow
const float mins[]={185,204,404};
const float maxs[]={492,497,643};

#endif

const float range_rad[] = { (maxs[0]-mins[0])*rad_per_level, (maxs[1]-mins[1])*rad_per_level,(maxs[2]-mins[2])*rad_per_level};
const float home_pose_rad[] = { range_rad[0], range_rad[1], 0};



template <typename T>
T clip(T x, T minx, T maxx){
	
	return min( max(x,minx) , maxx);
}





class robot{

public:

	void init(int myid){
		
		
		myprocid = myid;
		
		if( myid !=0 )return;  // Only proc 0 talks to the robot, but everyone else needs to know the proc id.
		
		
				
		
	};
		
	
	//******************************************************************************************************
	// Convert from raw robot motor angle data to radians from 0 to pi; also convert the velocities to rad/sec.
	// Since the max is 114 RPM, the max/min angular velocity is approx. +/- 11.9381 rad/sec.
	// shoulder = [200,512];
	// elbow    = [305,687);
	// deltoid  = [380,657];
	void do_state_conversions(){ 
		
		const float rpm_to_rad_s = 1.0f/60.0f*2*pi;
		
		// positions
		for( int i = 0; i < n_motors;i++){
			state_array[i] = rad_per_level* (state_array[i]-mins[i]);
		}
		// velocities can be negative or positive.
		for( int i = n_motors; i < 2*n_motors;i++){
			state_array[i] = state_array[i] * .111 * rpm_to_rad_s;
		}
		// torques can be negative or positive.
		for( int i = 2*n_motors; i < 3*n_motors;i++){
			state_array[i] = state_array[i] * (1.0/1024.0);
		}
		
	}
	
	
	
	
	//******************************************************************************************************
	// shoulder, deltoid, and elbow are the joint angles.  They will be in radians (0 to pi).
	//
	// sdot, ddot, and edot are the velocities of the above.  These will be in radians radians per second (range: +- 11.9381);
	//  
	void get_arm_proprioception(float &shoulder, float &deltoid, float &elbow, float &sdot, float &ddot, float &edot, float &storque, float &dtorque, float &etorque, int t){
		
		
		static bool first_call = true;
		
		
		if( t % frame_time_ms == 0 || first_call ){
			first_call = false; // just in case it's the first time.
			
			if( myprocid == 0 ){

				for(int i = 0; i < n_motors*3; i++ ){
					state_array[i] = (float)sim_state_array[i];
					cout << state_array[i] << " ";
				}
				cout << endl;
				
				do_state_conversions();
				
				//cout << state_array[0] << " " << state_array[1] << " " << state_array[2] << " " << state_array[3] << " " << state_array[4] << " " << state_array[5] << endl;
			}
			MPI_Bcast(state_array,n_motors*3, MPI_FLOAT, 0, network_comm ); // Proc 0 owns the data.
			
			
		}
		
		shoulder = state_array[0];
		deltoid = state_array[1];
		elbow = state_array[2];
		sdot = state_array[3];
		ddot = state_array[4];
		edot = state_array[5];
		storque = state_array[6];
		dtorque = state_array[7];
		etorque = state_array[8];
		
		/* could also get load info 
		 sdot = (float) narm_state.servo[SHOULDER]->load;
		 ddot = (float) narm_state.servo[DELTOID]->load;
		 edot = (float) narm_state.servo[ELBOW]->load;
		 */
		
	};
	
	
	//********************************************************************
	// joint angles are in radians from 0 to pi, and speeds (not directional) are in radians per second [0 to 11.9381].
	// Right now, the angles should be in the range 
	void set_arm_command(float shoulder, float deltoid, float elbow, float sdot, float ddot, float &edot, int t, int motor_update_interval_ms, int motor_update_offset_ms){
		
		// WARNING: if the mov_speed is ever set to 0, it means move at max speed!
		
		const int max_commanded_velocity = 75;
		const int torque_limit=512;
		const float levels_per_radian=1.0f/rad_per_level;
		const float rad_s_to_rpm = 60.0f/(2*pi);
		
		if( t % motor_update_interval_ms == motor_update_offset_ms ){
			
			if( myprocid == 0 ){
				
				shoulder = clip( shoulder*levels_per_radian+mins[0],mins[0],maxs[0]);
				deltoid  = clip( deltoid*levels_per_radian+mins[1],mins[1],maxs[1]);
				elbow    = clip( elbow*levels_per_radian+mins[2],mins[2],maxs[2]);
				
				// Make sure velocities are in range and convert to robot commands.
				
				goal_pos[0] = (int)shoulder;
				mov_speed[0] = (int) clip((int)(rad_s_to_rpm * 9.0090f * sdot), 1, max_commanded_velocity) ;
				torq_limit[0] = torque_limit;
				
				goal_pos[1] = (int)deltoid;
				mov_speed[1] = (int)(int) clip((int)(rad_s_to_rpm * 9.0090f * ddot), 1, max_commanded_velocity) ;
				torq_limit[1] = torque_limit;
				
				goal_pos[2] = (int)elbow;
				mov_speed[2] = (int) clip((int)(rad_s_to_rpm * 9.0090f * edot), 1, max_commanded_velocity) ;
				torq_limit[2] = torque_limit;
				
				// for now, the robot arm moves to the commanded position instantaneously.
				for(int i = 0; i < n_motors;i++){
					sim_state_array[i]=goal_pos[i];
					sim_state_array[i+n_motors]=mov_speed[i]; // velocities;
					sim_state_array[i+2*n_motors]=torq_limit[i]; // torques;
				}
			}
		}
	};	
	
	void quit(){

	};
	
	

private:
	
	
	float state_array[4*n_motors];  // Used to hold last set of sensor data received from the robot converted to correct units.
	int sim_state_array[4*n_motors];  // Used to hold last set of sensor data received from the robot.
	
	int myprocid;

	int goal_pos[n_motors];
	int mov_speed[n_motors]; 
	int torq_limit[n_motors];
	
};
